package org.nashua.tt151;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SimpleRobot;
import java.io.IOException;

public class OmniDrive extends SimpleRobot {
    Jaguar backLeft = new Jaguar(RobotMap.Jaguar.BACK_LEFT);
    Jaguar backRight = new Jaguar(RobotMap.Jaguar.BACK_RIGHT);
    Jaguar frontLeft = new Jaguar(RobotMap.Jaguar.FRONT_LEFT);
    Jaguar frontRight = new Jaguar(RobotMap.Jaguar.FRONT_RIGHT);
    Jaguar sideways = new Jaguar(RobotMap.Jaguar.SIDEWAYS);
    Controller.DualAction controller = new Controller.DualAction(1);
    Relay led = new Relay(RobotMap.Relay.LED);
    Gyro gyro = new Gyro(RobotMap.Analog.Gyro.SLOT);
    Dashboard dashboard;
    
    public void robotInit() {
        led.setDirection(Relay.Direction.kForward);
        led.set(Relay.Value.kOn);
        gyro.reset();
        gyro.setSensitivity(RobotMap.Analog.Gyro.SENSITIVITY);
        connectToDashboard();
    }
    public void connectToDashboard() {
        try {
            System.out.println("[ConnectToDashboard: Trying To Connect]");
            dashboard = new Dashboard("10.1.53.5", new Dashboard.ConnectionListener(){
                public void onDataRecieved(String str) {
                    if (str.equals("[Disconnect]")) {
                        dashboard = null;
                    }
                    System.out.println(str);
                }
            });
            System.out.println("[ConnectToDashboard: Connected Successfully]");
        } catch (Exception e) {
            System.out.println("[ConnectToDashboard: Failed To Connected]");
        }
    }
    public void disabled() {
        if (dashboard==null) {
            connectToDashboard();
        }
    }
    public void autonomous() {
        while (isEnabled()&&isAutonomous()) {
            System.out.println(gyro.getAngle());
        }
    }
    public void operatorControl() {
        while (isEnabled()&&isOperatorControl()) {
            // Movement
            double x = controller.getRightX();
            double y = controller.getRightY()*0.5;
            if(controller.getLeftBumper()){
                backLeft.set(.5);
                frontLeft.set(.5);
                backRight.set(.5);
                frontRight.set(.5);
            }else if(controller.getRightBumper()){
                backLeft.set(-.5);
                frontLeft.set(-.5);
                backRight.set(-.5);
                frontRight.set(-.5);
            }else{
                backLeft.set(y);
                frontLeft.set(y);
                backRight.set(-y);
                frontRight.set(-y);
                sideways.set(-x);
            }
            // LED
            if (controller.getBack()) {
                led.set(Relay.Value.kOff);
            }
            else if (controller.getSelect()) {
                led.set(Relay.Value.kOn);
            }
            // Gyro
            if (controller.getLeftTrigger()) {
                gyro.reset();
            }
            System.out.println("Gyro: "+((int)(gyro.getAngle()*1000))/1000.0);
            // AutoMove
            if (controller.getButtonA()) {
                autoMove(5,0);
            }
        }
    }
    public void autoMove(int feet, double inches) {
        if (dashboard!=null) {
            //setPerpendicular();
            setDistance(feet+inches/12);
            //setPerpendicular();
            //setCenter();
            //setPerpendicular();
        }
    }

    private void setPerpendicular(){ 
        double power = .5;
        for (int i=0; i<3; i++) {
            double angle = -gyro.getAngle()%360;
            if (angle<0) {
                angle = 360-angle;
            }
            angle -= 180;
            while ((angle>RobotMap.Analog.Gyro.THRESHOLD || angle<-RobotMap.Analog.Gyro.THRESHOLD) && !controller.getLeftBumper()){
                double pid = power*angle/180;
                if (angle>RobotMap.Analog.Gyro.THRESHOLD) {
                    backLeft.set(pid);
                    frontLeft.set(pid);
                    backRight.set(pid);
                    frontRight.set(pid);
                } else if (angle<-RobotMap.Analog.Gyro.THRESHOLD) {
                    backLeft.set(pid);
                    frontLeft.set(pid);
                    backRight.set(pid);
                    frontRight.set(pid);
                }
                try {
                    wait(1);
                } catch (Exception e) {
                }
                backLeft.set(0);
                frontLeft.set(0);
                backRight.set(0);
                frontRight.set(0);
                angle = -gyro.getAngle()%360;
                if (angle<0) {
                    angle = 360-angle;
                }
                angle -= 180;
            }
        }
    }

    private void setDistance(double d) {
        try {
            double dist = 0;
            while ((dist=Double.parseDouble(dashboard.requestInfo("DIS BOT")))!=d) {
                if (dist==-1) return;
                double speed=dist>d ? 1 : -1;
                backLeft.set(speed);
                frontLeft.set(speed);
                backRight.set(-speed);
                frontRight.set(-speed);
                try {
                    wait(1);
                } catch (Exception e) {
                }
                backLeft.set(0);
                frontLeft.set(0);
                backRight.set(0);
                frontRight.set(0);
            }
        } catch (IOException ex) {
            ex.printStackTrace();
        }
    }
    
    private void setCenter() {
    }
}
